import image, math, time
import os
import sys
import binascii
import ustruct

from media.sensor import *
from media.display import *
from media.media import *
from time import ticks_ms
from machine import FPIOA
from machine import Pin
from machine import UART

sensor = None

uart = UART(UART.UART2, 115200)

def send_two_uchar(c1, c2):
    global uart
    header = 0xAA
    # 由于每个int拆分为2个uint8_t，总数据长度变为4（c1高8位、c1低8位、c2高8位、c2低8位）
    total_bits = 6  # 数据部分总字节数：header(1) + total_bits(1) + 4个数据字节 + checksum(1) = 8字节，此处total_bits表示数据段长度
    # 将int拆分为高8位和低8位（uint8_t）
    c1_high = (c1 >> 8) & 0xFF
    c1_low = c1 & 0xFF
    c2_high = (c2 >> 8) & 0xFF
    c2_low = c2 & 0xFF
    data_parts = [header, total_bits, c1_high, c1_low, c2_high, c2_low]
    data = ustruct.pack("<BBBBBB", *data_parts)  # 对应6个字节
    checksum = sum(data_parts) & 0xFF  # 限制为uint8_t范围
    data += ustruct.pack("<B", checksum)
    if uart is not None:
        uart.write(data)
        hex_data = binascii.hexlify(data).decode('utf-8')
        print(f"发送16进制数据:{hex_data}")
    return data

try:
    print("camera_test")
    fpioa = FPIOA()
    fpioa.help()
    fpioa.set_function(33, FPIOA.GPIO33)
    fpioa.set_function(11, FPIOA.UART2_TXD)
    fpioa.set_function(12, FPIOA.UART2_RXD)

    pin = Pin(33, Pin.OUT)
    pin.value(0)


    sensor = Sensor(width=640, height=480)
    sensor.reset()

    sensor.set_framesize(width=640, height=480)
    sensor.set_pixformat(Sensor.RGB565)

    Display.init(Display.LT9611, width=640, height=480, to_ide=True)

    MediaManager.init()

    sensor.run()
    clock = time.clock()

    pin.value(1)
    flag = 0
    center_x = 0
    center_y = 0
    delta_x, delta_y = 0, 0

    while True:
        clock.tick()
        os.exitpoint()
        img = sensor.snapshot(chn=CAM_CHN_ID_0)


         ##=矩形识别，可以用来找矩形的四个角的坐标
        img_rect = img.to_grayscale(copy=True)
        img_rect = img_rect.binary([(82, 212)])
        rects = img_rect.find_rects(threshold=100000)

        if not rects == None:
            for rect in rects:
                corner = rect.corners()
                img.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[2][0], corner[2][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[0][0], corner[0][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                center_x = sum([corner[k][0] for k in range(4)])//4
                center_y = sum([corner[k][1] for k in range(4)])//4
                x = 320
                y = 240
                delta_x = center_x - x
                delta_y = center_y - y

                print(delta_x, delta_y)

        img.draw_string_advanced(50, 50, 40, "fps: {}".format(clock.fps()), color=(255, 0, 0))
        img.midpoint_pool(2, 2)
        img.compressed_for_ide()
        Display.show_image(img, x=640, y=480)


except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    if isinstance(sensor, Sensor):
        sensor.stop()
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    MediaManager.deinit()
